package frsf.cidisi.exercise.modelo.search;

import java.util.List;

import frsf.cidisi.exercise.modelo.util.Coordenada;
import frsf.cidisi.exercise.modelo.util.Salida;
import frsf.cidisi.faia.solver.search.IEstimatedCostFunction;
import frsf.cidisi.faia.solver.search.NTree;

/**
 * This class allows to define a function to be used by any
 * informed search strategy, like A Star or Greedy.
 */
public class Heuristic implements IEstimatedCostFunction {

    /**
     * It returns the estimated cost to reach the goal from a NTree node.
     */
    @Override
    public double getEstimatedCost(NTree node) {
        AgenteEstado agState = (AgenteEstado) node.getAgentState();
        
        Coordenada posicion = agState.getPosAgente();        
        List<Salida> salidas = agState.getSalida();
        double menor = 99;
        double aux = 0;
        
        	
        	for(Salida salida : salidas){
        			aux = distancia(posicion, salida.getCoordenada());
        		
        		if(aux < menor)
        			menor = aux;
        	}
        
        return menor;
		
    }
    
    private double distancia(Coordenada p1, Coordenada p2){
    	double respuest = 0;
        double res1,res2;
        int x1, x2, y1, y2;
        
        x1 = p1.getX().intValue();
        x2 = p2.getX().intValue();
        y1 = p1.getY().intValue();
        y2 = p2.getY().intValue();
        
        res1 = x2 - x1;
        res2 = y2 - y1;
        
        res1 = (res1*res1) + (res2*res2);
        respuest=Math.sqrt(res1);
        
        return respuest;
    }
}
